﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using xn;
using System.Diagnostics;
using MindDrone.Common;

namespace Kinect.MotionDetection
{
    public class MovementDetection
    {
        public event EventHandler<MovementEventArgs> OnMovementChanged;

        
        //TODO - do this better : distance stuff - 

        private void MovementChange(MovementType type)
        {
            //Tracer.Log(type.ToString());
            if (OnMovementChanged != null)
                OnMovementChanged(this, new MovementEventArgs(type));
        }
        public void CheckMouvements(Point3D headPos, Point3D leftPos, Point3D rightPos)
        {
            //Tracer.Log("Head:" + "(" + (int)headPos.X + ":" + (int)headPos.Y + ")" +
            //                 " left:" + "(" + (int)leftPos.X + ":" + (int)leftPos.Y + ")" +
            //                 " right:" + "(" + (int)rightPos.X + ":" + (int)rightPos.Y + ")");
            //Tracer.Log("   ---> CALCUL1 :" + (Math.Pow(((leftPos.X - rightPos.X) / rightPos.X), 2) + Math.Pow(((leftPos.Y - rightPos.Y) / rightPos.Y), 2)));

            double angleLeft = MovementDetection.CalculateAngle(0, 0, leftPos.X, leftPos.Y);
            double angleRight = MovementDetection.CalculateAngle(0, 0, rightPos.X, rightPos.Y);

            if ((160 <= angleLeft && angleLeft <= 190) && (270 <= angleRight && angleRight <= 300))
            {
                MovementChange(MovementType.LEFT);
                return;
            }

            if ((230 <= angleLeft && angleLeft <= 260) && ((340 <= angleRight && angleRight <= 360) || (0<= angleRight && angleRight<= 15)))
            {
                MovementChange(MovementType.RIGHT);
                return;
            }

            if ((145 <= angleLeft && angleLeft <= 190) && ((340 <= angleRight && angleRight <= 360) || (0 <= angleRight && angleRight <= 25)))
            {
                if (leftPos.Z - rightPos.Z <= 200 && leftPos.Z - rightPos.Z >= -200 && ( headPos.Z - rightPos.Z >= 390 || headPos.Z - leftPos.Z >= 390) )
                {
                    //now test if STRAIGHT FORWARD
                    MovementChange(MovementType.STRAIGHT_FORWARD);
                    return;
                }
                else
                {
                    //specific to get accurate straight or not
                    if ((160 <= angleLeft && angleLeft <= 190) && ((340 <= angleRight && angleRight <= 360) || (0 <= angleRight && angleRight <= 15)))
                    {
                        MovementChange(MovementType.STRAIGHT);
                        return;
                    }
                }
               
            }
            if (110 <= angleLeft && angleLeft <= 160 && 40 <= angleRight && angleRight <= 80 && rightPos.Y - headPos.Y>= 200 && leftPos.Y - headPos.Y >= 200 )
            {
                MovementChange(MovementType.STRAIGHT_UPWARD);
                return;
            }
            if ((190 <= angleLeft && angleLeft <= 260) && (270 <= angleRight && angleRight <= 350) && headPos.Z - rightPos.Z <= 200  &&  headPos.Z- leftPos.Z <= 200)
            {
                if (headPos.Z - rightPos.Z <= -380 && headPos.Z - leftPos.Z <= -450)
                    MovementChange(MovementType.STRAIGHT_BACKWARD);
                else
                    MovementChange(MovementType.STRAIGHT_DOWNWARD);
                return;
            }
            //// If the user group his hand (above his head for instance)
            //if ((Math.Pow(((leftPos.X - rightPos.X) / rightPos.X), 2) + Math.Pow(((leftPos.Y - rightPos.Y) / rightPos.Y), 2)) < 0.05)
            //{
            //    MovementChange(MovementType.GROUPING_HAND);
            //    return;
            //}// If the user group his hand (above his head for instance)
            if (leftPos.X > headPos.X && rightPos.X < headPos.X)
            {
                MovementChange(MovementType.GROUPING_HAND);
                return;
            }
            
            // If the user is out of the camera field
            //if (Math.Pow(((headPos.X+leftPos.X+rightPos.X)/3),2) >  1000000)
            //{
            //    MovementChange(MovementType.OUT);
            //    return;
            //}

            MovementChange(MovementType.NOTHING);
        }

        public static double CalculateAngle(double px1, double py1, double px2, double py2)
        {
            // Negate X and Y values
            double pxRes = px2 - px1;
            double pyRes = py2 - py1;
            double angle = 0.0;
            // Calculate the angle
            if (pxRes == 0.0)
            {
                if (pxRes == 0.0)
                    angle = 0.0;
                else if (pyRes > 0.0) angle = System.Math.PI / 2.0;
                else
                    angle = System.Math.PI * 3.0 / 2.0;
            }
            else if (pyRes == 0.0)
            {
                if (pxRes > 0.0)
                    angle = 0.0;
                else
                    angle = System.Math.PI;
            }
            else
            {
                if (pxRes < 0.0)
                    angle = System.Math.Atan(pyRes / pxRes) + System.Math.PI;
                else if (pyRes < 0.0) angle = System.Math.Atan(pyRes / pxRes) + (2 * System.Math.PI);
                else
                    angle = System.Math.Atan(pyRes / pxRes);
            }
            // Convert to degrees
            angle = angle * 180 / System.Math.PI; return angle;

        }
    }
}
